/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.controller.breakaway.teleop;

import com.grt192.core.EventController;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.ButtonListener;
import com.grt192.event.component.JoystickEvent;
import com.grt192.event.component.JoystickListener;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.mechanism.breakaway.KickerOmega;
import com.grt192.sensor.GRTJoystick;
import com.grt192.sensor.GRTSwitch;
import edu.wpi.first.wpilibj.templates.MainRobot;

/**
 * 
 * @author GRTstudent
 */
public class KickerOmegaController extends EventController implements
		ButtonListener, SensorChangeListener, JoystickListener {

	public static final String CAMERA_UP_BUTTON = "Button4";
	public static final String CAMERA_DOWN_BUTTON = "Button1";
	public static final String KICKER_FORWARD_BUTTON = "Button5";
	public static final String KICKER_REVERSE_BUTTON = "Button6";
	public static final double KICK_THRESHOLD = -.3;
	public static final double OVERRIDE_THRESHOLD = .3;

	public static final double DISABLE_TIME = 2500;
	private double lastFired;
	private KickerOmega kicker;
	private boolean override;

	public KickerOmegaController(GRTDriverStation ds, KickerOmega k) {
		addMechanism("Kicker", k);
		addMechanism("DriverStation", ds);
		((GRTJoystick) (ds.getSensor("secondaryJoystick")))
				.addButtonListener(this);
		((GRTJoystick) (ds.getSensor("secondaryJoystick")))
				.addJoystickListener(this);
		((GRTJoystick) (ds.getSensor("rightJoystick")))
				.addButtonListener(this);
		((GRTSwitch) k.getSensor("LimitSwitch")).addSensorChangeListener(this);
		kicker = k;
		lastFired = 0;
		override = false;
	}

	public void buttonUp(SensorEvent e, String key) {
		if (key.equals(KICKER_FORWARD_BUTTON)
				|| key.equals(KICKER_REVERSE_BUTTON)) {
			kicker.stopKick();
		}
		if (key.equals(KICKER_FORWARD_BUTTON) && override) {
			MainRobot.getInstance().putGlobal("KickerDirection", "Stopped");
		}
		if (key.equals(KICKER_REVERSE_BUTTON) && override) {
			MainRobot.getInstance().putGlobal("KickerDirection", "Stopped");
		}
		if (key.equals("Button4") && e.getSource().getId().equals("right")) {
			kicker.kick();
			lastFired = System.currentTimeMillis();
			System.out.println("Kicking");
		}
	}

	public void buttonDown(SensorEvent e, String key) {
		if (key.equals(KICKER_FORWARD_BUTTON) && override) {
			kicker.forward();
			System.out.println("Forward");
			MainRobot.getInstance().putGlobal("KickerDirection", "Foward");
		}
		if (key.equals(KICKER_REVERSE_BUTTON) && override) {
			kicker.reverse();
			System.out.println("Reverse");
			MainRobot.getInstance().putGlobal("KickerDirection", "Backward");
		}
	}

	public void sensorStateChanged(SensorEvent e, String key) {
		boolean switchHit = e.getData(key) == KickerOmega.SWITCH_HIT;
		System.out.println("Limit Switch hit");
		if (switchHit && kicker.isKicking()) {
			kicker.stopKick();
			MainRobot.getInstance().putGlobal(key, new Boolean(switchHit));
		} else {
			MainRobot.getInstance().putGlobal("Kicking", new Boolean(true));
			MainRobot.getInstance().putGlobal(key, new Boolean(switchHit));
		}
	}

	public void xAxisMoved(JoystickEvent e) {
	}

	public void yAxisMoved(JoystickEvent e) {
	}

	public void zAxisMoved(JoystickEvent e) {
		if (e.getValue() < KICK_THRESHOLD && !kicker.isKicking()
				&& System.currentTimeMillis() - lastFired > DISABLE_TIME) {
			kicker.kick();
			lastFired = System.currentTimeMillis();
			System.out.println("Kicking");
		}
		if (e.getValue() > OVERRIDE_THRESHOLD) {
			if (!override) {
				System.out.println("Override on");
			}
			override = true;
		} else {
			if (override) {
				System.out.println("Override off");
				MainRobot.getInstance().putGlobal("KickerDirection", "Stopped");
			}
			override = false;
		}
	}

	public void throttleMoved(JoystickEvent e) {
	}
}
